#include "gaitStep.h"
#include <fstream>
#include <iostream>

using namespace Eigen;
GaitStep::GaitStep(string n,int len)
{
    //ctor
    length=0;
    name=n;
    /*
    m_data=new int[len*20];
    m_gait=Map<matrix>(m_data,len,20);*/
    //m_gait=matrix(len,20);
    m_data=new int[len*20];
    max_size=len;
}

GaitStep::~GaitStep()
{
    //dtor
    if(m_data)
    delete [] m_data;
    m_data=0;
}

 bool GaitStep::pushData(int *d)
{
    int i=0;
    if(length>=max_size)
    {
        max_size=2*max_size+1;
        int *t_p=new int[max_size*20];
        copyDataBlock(t_p,m_data,20,length,1);
        delete [] m_data;
        m_data=t_p;
    }
    if(d)
    copyData(m_data+length*20,d,20,1);
    length++;
    return true;
}
bool GaitStep::popData(int *d)
{
    if(length==0)
        return false;
    int i=0;
    copyData(d,m_data+length*20,20,1);
    length--;
    return true;
}
void GaitStep::getData(motorArray d,int i)
{
    copyData(d,m_data+i*20,20,1);
}
bool GaitStep::loadFile(string f)
{
    step=0;
    std::ifstream file(("./gaits/"+f).c_str());
    if(!file)
        return false;
    int d;
    int i=0,j=0;
    motorArray tmp;
    while(file>>d)
    {
        tmp[i++]=d;
        if(i>=20)
        {
            j++;
            i=0;
            if(!pushData(tmp))
            {
                return false;
            }

        }
    }

    file.close();
    return true;
}
bool GaitStep::saveFile()
{
    std::ofstream file((("./gaits/"+name).c_str()));

    file<<getMatrix();
    file.close();

    return true;
}
matrix GaitStep::getMatrix()
{
    return Map<matrix>(m_data,length,20);
}
